package MULE.Bewegungsmodul;

import lejos.nxt.NXTRegulatedMotor;
import lejos.robotics.BaseMotor;
/**import lejos.util.PIDController;
import lejos.util.Timer;
import lejos.util.TimerListener;
*/
public class PIDMotor implements BaseMotor{
	//private PIDController pid;
	private NXTRegulatedMotor motor;
	private int setpoint = 90;
	private float deadband = 1;
	private float kp = 3.5f;
	private float ki = 0.0f;
	private float kd = 0.0f;
	private float limithigh = 200.0f;
	private float limitlow = -200.0f;
	private float integrallimithigh = 0.0f;
	private float integrallimitlow = 0.0f;
	private int dt = 1000;
	   
	/**
	 * PID controlled
	 * 
	 * @param port
	 */
	public PIDMotor(NXTRegulatedMotor motor){
		this.motor = motor;
		/**
		pid = new PIDController(this.setpoint);
        Timer timer = new Timer(this.getDt(), this);
        timer.start();
        
        pid.setPIDParam(PIDController.PID_DEADBAND, getDeadband());
        pid.setPIDParam(PIDController.PID_KP, getKp());
        pid.setPIDParam(PIDController.PID_KI, getKi());
        pid.setPIDParam(PIDController.PID_KD, getKd());
        pid.setPIDParam(PIDController.PID_LIMITHIGH, getLimithigh());
        pid.setPIDParam(PIDController.PID_LIMITLOW, getLimitlow());
        pid.setPIDParam(PIDController.PID_I_LIMITHIGH, getIntegrallimithigh());
        pid.setPIDParam(PIDController.PID_I_LIMITLOW, getIntegrallimitlow());
        
        */
	}
	
	/**
	 * PID controlled
	 */
	public void forward(){
		this.motor.forward();
	}
	
	/**
	 * PID controlled
	 */
	public void backward() {
		this.motor.backward();
	}
	
	/**
	 * PID controlled setSpeed
	 * 
	 * @param speed
	 */
	public void setSpeed(float speed){
		motor.setSpeed(speed);
		//double actSpeed = motor.getSpeed();
		
		//pid.setPIDParam(PIDController.PID_SETPOINT, (int)speed);
      
		//motor.setSpeed(pid.doPID((int)actSpeed));
	}
	
	/**
	 * PID controlled
	 */
	public int getSpeed(){
		return (int)this.getSetpoint();
	}

	public void setSetpoint(float setpoint) {
		this.setpoint = (int) setpoint;
	}

	public int getTachoCount(){
		return motor.getTachoCount();
	}
	
	public float getSetpoint() {
		return setpoint;
	}

	public void setDeadband(float deadband) {
		this.deadband = deadband;
	}

	public float getDeadband() {
		return deadband;
	}

	public void setKp(float kp) {
		this.kp = kp;
	}

	public float getKp() {
		return kp;
	}

	public void setKi(float ki) {
		this.ki = ki;
	}

	public float getKi() {
		return ki;
	}

	public void setKd(float kd) {
		this.kd = kd;
	}

	public float getKd() {
		return kd;
	}

	public void setLimithigh(float limithigh) {
		this.limithigh = limithigh;
	}

	public float getLimithigh() {
		return limithigh;
	}

	public void setLimitlow(float limitlow) {
		this.limitlow = limitlow;
	}

	public float getLimitlow() {
		return limitlow;
	}

	public void setIntegrallimithigh(float integrallimithigh) {
		this.integrallimithigh = integrallimithigh;
	}

	public float getIntegrallimithigh() {
		return integrallimithigh;
	}

	public void setIntegrallimitlow(float integrallimitlow) {
		this.integrallimitlow = integrallimitlow;
	}

	public float getIntegrallimitlow() {
		return integrallimitlow;
	}

	public void setDt(int dt) {
		this.dt = dt;
	}

	public int getDt() {
		return dt;
	}

	public void flt() {
		// TODO Auto-generated method stub
		
	}

	public boolean isMoving() {
		return this.motor.isMoving();
	}

	public void stop() {
		this.motor.stop();
	}

	/**
	public void timedOut() {
		double actSpeed = this.motor.getSpeed();
		System.out.println(actSpeed);
		System.out.println(pid.doPID((int)actSpeed));
		Button.waitForPress();
		this.motor.setSpeed(pid.doPID((int)actSpeed));
	}
	*/
}

	
